# -*- coding: utf-8 -*-
import numpy as np
# import matplotlib.pyplot as plt
IN=4;H=5;Out=3;  #NN Structure
wi = np.mat('-0.2846    0.2193   -0.5097   -1.0668;-0.7484   -0.1210   -0.4708    0.0988;-0.7176    0.8297   -1.6000    0.2049;-0.0858    0.1925   -0.6346    0.0347;0.4358    0.2369   -0.4564   -0.1324');
wo = np.mat('1.0438    0.5478    0.8682    0.1446    0.1537;0.1716    0.5811    1.1214    0.5067    0.7370;1.0063    0.7428    1.0534    0.7824    0.6494');
ts=0.01
y_1= 0
u_1 = 0
error_1 = 0
error_2 = 0
du_1 = 0
wo_1 = wo
wo_2 = wo
wi_1=wi
wi_2=wi
wi_3=wi

MAX_NUM = 3000

rin = np.zeros(MAX_NUM)
a = np.zeros(MAX_NUM)
x = np.zeros(3)
error = np.zeros(MAX_NUM)
yout = np.zeros(MAX_NUM)
kp = np.zeros(MAX_NUM)
ki = np.zeros(MAX_NUM)
kd = np.zeros(MAX_NUM)
du = np.zeros(MAX_NUM)
u = np.zeros(MAX_NUM)
dyu = np.zeros(MAX_NUM)
dK = np.zeros(MAX_NUM)
delta3 = np.zeros(3)
dO = np.zeros(5)
delta2 = np.zeros(5)
x = np.zeros(3)
Oh=np.mat('0.0 ;0.0 ;0.0 ;0.0 ;0.0')
xite=0.20;
alfa=0.05;
for k in range(0,MAX_NUM):
    rin[k]=np.sin(1*2*np.pi*(k+1)*ts)
    a[k]=1.2*(1-0.8*np.exp(-0.1*(k+1)))
    yout[k]=a[k]*y_1/(1+y_1**2)+u_1
    # print yout[k]
    error[k]=rin[k]-yout[k]
    xi=[rin[k],yout[k],error[k],1]
    x[0]=error[k]-error_1
    x[1]=error[k]
    x[2]=error[k]-2*error_1+error_2
    epid=[x[0],x[1],x[2]]
    I=(xi*(wi.T))
    for j in range(H):
        Oh[j,0]=(np.exp(I[0,j])-np.exp(-I[0,j]))/(np.exp(I[0,j])+np.exp(-I[0,j])) #Middle Layer
    # print Oh.shape
    # print wo.shape
    #size(Oh)
    K = wo*(Oh)             #Output Layer
    # print 'k:',K
    for l in range(Out):
        K[l]=np.exp(K[l])/(np.exp(K[l])+np.exp(-K[l]))        #Getting kp,ki,kd sigmoid 限幅
        # print "K[%d]=%f"%(l,K[l])
    kp[k]=K[0]
    ki[k]=K[1]
    kd[k]=K[2]
    Kpid=[kp[k],ki[k],kd[k]]

    du[k]=np.dot(Kpid,epid)  #pid的增量值
    u[k]=u_1+du[k] #pid的输出值

    dyu[k] = np.sign((yout[k]-y_1)/(du[k]-du_1+0.0001))  #y对du的符号函数用于替代y对du的偏导数
    for j in range(Out):
        dK[j]=2/((np.exp(K[j])+np.exp(-K[j]))**2)

    for l in range(Out):
        delta3[l]=error[k]*dyu[k]*epid[l]*dK[l]

    d_wo = 0.0
    for l in range(Out):
        for i in range(H):
            d_wo=xite*delta3[l]*Oh[i,0]
            d_wo = np.add(np.dot(alfa,(wo_1-wo_2)),d_wo)

    wo=wo_1+d_wo+alfa*(wo_1-wo_2)#输出层权值更新
    #Hidden layer
    for i in range(H):
        dO[i]=4/((np.exp(I[0,i])+np.exp(-I[0,i]))**2)

    segma=delta3*wo
    for i in range(H):
        delta2[i]=dO[i]*segma[0,i]


    d_wi=np.dot(xite,delta2)
    d_wi = np.mat(d_wi).T*np.mat(xi)
    wi=wi_1+d_wi+np.dot(alfa,(wi_1-wi_2))

    #Parameters Update
    du_1=du[k]
    # u_5=u_4
    # u_4=u_3
    # u_3=u_2
    u_2=u_1
    u_1=u[k]
    y_2=y_1;y_1=yout[k]

    wo_3=wo_2
    wo_2=wo_1
    wo_1=wo

    wi_3=wi_2
    wi_2=wi_1
    wi_1=wi

    error_2=error_1
    error_1=error[k]
t = np.arange(MAX_NUM)
# plt.figure(1)
# plt.plot(t,rin,'r',t,yout,'b')
# plt.figure(2)
# plt.plot(t,yout,'b')
# plt.figure(3)
# plt.plot(t,u,'r')
# plt.show()
t = np.arange(6000)